#include "socket.h"
#include <cstring>

#include <ros/ros.h>

Socket::Socket(int port)
{
	mSocket = NULL;

	if(!(mSocketSet = SDLNet_AllocSocketSet(1)))
	{
		ROS_ERROR("SDLNet_AllocSocketSet: %s", SDLNet_GetError());
		return;
	}

	if (SDLNet_ResolveHost(&mIP, NULL, port) < 0)
	{
		ROS_ERROR("SDLNet_ResolveHost: %s: when resolving ip for port: %d", SDLNet_GetError(), port);
		return;
	}

	/* Open a listening TCP port */
	if (!(mSocket = SDLNet_TCP_Open(&mIP)))
	{
		ROS_ERROR("SDLNet_TCP_Open: %s: listening port: %d", SDLNet_GetError(), port);
		return;
	}

	SDLNet_TCP_AddSocket(mSocketSet, mSocket);

	ROS_INFO("Socket: Listening on port %d", port);
}

Socket::Socket(string hostname, int port)
{
	mSocket = NULL;

	if(!(mSocketSet = SDLNet_AllocSocketSet(1)))
	{
		ROS_ERROR("SDLNet_AllocSocketSet: %s", SDLNet_GetError());
		return;
	}

	/* Resolve the host we are connecting to */
	if (SDLNet_ResolveHost(&mIP, hostname.c_str(), port) < 0)
	{
		ROS_ERROR("SDLNet_ResolveHost: %s: %s:%d", SDLNet_GetError(), hostname.c_str(), port);
		return;
	}

	/* Open a TCP connection */
	if (!(mSocket = SDLNet_TCP_Open(&mIP)))
	{
		ROS_ERROR("SDLNet_TCP_Open: %s: %s:%d", SDLNet_GetError(), hostname.c_str(), port);
		return;
	}

	SDLNet_TCP_AddSocket(mSocketSet, mSocket);

	ROS_INFO("Socket: Connected to remote host %s on port %d", hostname.c_str(), port);
}

Socket::Socket(TCPsocket sock)
{
	if(!(mSocketSet = SDLNet_AllocSocketSet(1)))
	{
		ROS_ERROR("SDLNet_AllocSocketSet: %s", SDLNet_GetError());
		return;
	}

	mSocket = sock;

	mIP = *SDLNet_TCP_GetPeerAddress(sock);
	string hostname = SDLNet_ResolveIP(&mIP);

	SDLNet_TCP_AddSocket(mSocketSet, mSocket);

	ROS_INFO("Socket: Connected to remote host %s on port %d", hostname.c_str(), SDLNet_Read16(&mIP.port));
}

Socket::~Socket()
{
	if(mSocketSet)
		SDLNet_FreeSocketSet(mSocketSet);
}

void Socket::Init()
{
	ROS_INFO("Initializing sockets");
	if(SDLNet_Init() < 0)
		ROS_ERROR("SDLNet_Init() error: %s", SDLNet_GetError());
}

void Socket::Shutdown()
{
	SDLNet_Quit();
}

Socket* Socket::Accept()
{
	TCPsocket sock;

	sock = SDLNet_TCP_Accept(mSocket);

	if(sock)
		return new Socket(sock);
	else
		return NULL;
}

bool Socket::Recv(u_int8_t *buffer, size_t &len, bool wait)
{
	if(wait)
	{
		len = SDLNet_TCP_Recv(mSocket, buffer, 1024);
		ROS_DEBUG("Received %d bytes of data", (int)len);
		return (len != 0);
	}

	// Handle incoming messages
	while(SDLNet_CheckSockets(mSocketSet, 0) > 0)
	{
		if(SDLNet_SocketReady(mSocket))
		{
			len = SDLNet_TCP_Recv(mSocket, buffer, 1024);
			ROS_DEBUG("Received %d bytes of data", (int)len);
			return (len != 0);
		}
	}

	return false;
}

bool Socket::Send(u_int8_t *buffer, size_t len)
{
	int result = SDLNet_TCP_Send(mSocket, buffer, len);
	ROS_DEBUG("Sending %d bytes of data", (int)len);
	return (result == len);
}
